#!/usr/bin/env python3

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node, SetParameter

def generate_launch_description():
    # 获取包路径
    pb2025_nav_bringup_dir = get_package_share_directory('pb2025_nav_bringup')
    
    # 声明参数
    use_sim_time = LaunchConfiguration('use_sim_time')
    namespace = LaunchConfiguration('namespace')
    
    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation clock if true'
    )
    
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace'
    )
    
    # 高度控制节点
    altitude_controller_cmd = Node(
        package='pb2025_nav_bringup',
        executable='altitude_controller.py',
        name='altitude_controller',
        namespace=namespace,
        output='screen',
        parameters=[{
            'use_sim_time': use_sim_time,
        }],
        remappings=[
            ('/target_altitude', '/target_altitude'),
        ]
    )
    
    # 创建launch描述
    ld = LaunchDescription()
    
    # 添加参数声明
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_namespace_cmd)
    
    # 添加节点
    ld.add_action(altitude_controller_cmd)
    
    return ld 